{
 "cells": [
  {
   "cell_type": "markdown",
   "source": [
    "## Создание матрицы Денавита-Хартенберга\n"
   ],
   "metadata": {
    "collapsed": false
   }
  },
  {
   "cell_type": "markdown",
   "source": [
    "#### Импорты"
   ],
   "metadata": {
    "collapsed": false
   }
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "import math\n",
    "\n",
    "import numpy as np\n",
    "import sympy as sy\n",
    "\n",
    "from robobase.dhmatrix import DHMatrix\n",
    "from src.support import toNumpy"
   ]
  },
  {
   "cell_type": "markdown",
   "source": [
    "#### Символьный вид"
   ],
   "metadata": {
    "collapsed": false
   }
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "⎡cos(θ)  -sin(θ)⋅cos(a)  sin(a)⋅sin(θ)   r⋅cos(θ)⎤\n",
      "⎢                                                ⎥\n",
      "⎢sin(θ)  cos(a)⋅cos(θ)   -sin(a)⋅cos(θ)  r⋅sin(θ)⎥\n",
      "⎢                                                ⎥\n",
      "⎢  0         sin(a)          cos(a)         d    ⎥\n",
      "⎢                                                ⎥\n",
      "⎣  0           0               0            1    ⎦\n"
     ]
    }
   ],
   "source": [
    "r, a, d, theta = sy.symbols('r'), sy.symbols('a'), sy.symbols('d'), sy.symbols('theta')\n",
    "m = DHMatrix.matrix(r, a, d, theta)\n",
    "sy.pprint(m)"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   }
  },
  {
   "cell_type": "markdown",
   "source": [
    "#### Численный вид"
   ],
   "metadata": {
    "collapsed": false
   }
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[ 0. -1.  0.  0.]\n",
      " [ 1.  0.  0.  5.]\n",
      " [ 0.  0.  1.  3.]\n",
      " [ 0.  0.  0.  1.]]\n"
     ]
    }
   ],
   "source": [
    "m = m.evalf(subs={r: 5, a: 0, d: 3, theta: math.pi / 2})\n",
    "print(np.round(toNumpy(m), 2))"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   }
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 2
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython2",
   "version": "2.7.6"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 0
}